#include "FFTDisp.h"
#include <FHT.h>

int                       oct_bias[OCTAVES];      // the bias for an empty signal per octave
int                       oct_averages[OCTAVES];  // the average over SAMPLE_COUNT for the current input
int                       count = 0;              // the number of ffts since the last averaging operation
uint16_t                  countSinceAverage = 0;
float                     oct_localAvgCalc[OCTAVES];
float                     oct_localAvg[OCTAVES];
uint16_t                  oct_globalMax[OCTAVES];
int                       readOffset;


void initDisp() {
  ADCSRA = 0xe5; // set the adc to free running mode
  ADMUX = 0x40; // use adc0
  DIDR0 = 0x03; // turn off the digital input for adc0 & adc1
 clearGlobal();
  avgCalib();
  clearAverages();

  if(readInt16FromEEprom(0) != 1) {
    calibrate();
  }

  // read calibration values
  readOffset = readInt16FromEEprom(1);
  for(int i = 0; i < OCTAVES; i++) {
    oct_bias[i] = readInt16FromEEprom(i+2);
  }
  
  printCalibration();

}


void calibrate() {
  Serial.println("Calibrating");
  readOffset = 0;
  double newOffset = 0;
  for(int i = 0; i < AVERAGE_COUNT; i++) {
    readInput();
    // determine the total average value
    uint32_t sum = 0;
    for(int j = 0; j < FHT_N; j++) {
      sum += (((uint16_t)fht_input[j])>>6);
    }
    sum /= FHT_N;
    newOffset = newOffset + ((float)(sum-newOffset)/((float)i+1));
  }
  readOffset = newOffset;
  writeInt16ToEEPRom(1,readOffset);

  // determine the octave bias
  avgCalib();
  for(int i = 0; i < OCTAVES; i++ ) {
    oct_bias[i] = oct_averages[i];
    writeInt16ToEEPRom(i+2,oct_bias[i]);
  }
  writeInt16ToEEPRom(0,1);
  printCalibration();
}


void printCalibration() {
  Serial.print("ReadOffset: ");
  Serial.println(readOffset);
  Serial.print("Oct Biases: ");
  for(int i = 0; i < OCTAVES; i++) {
    Serial.print(oct_bias[i]);
    Serial.print(" ");
  }
  Serial.println();
}


bool displayFFT(LegData *data) {
    bool clipped = fht();
    averageOutput();
    if(count == SAMPLE_COUNT) {
      for(int i = 0; i < OCTAVES; i++) {
        int minOct = oct_localAvg[i];
        int maxOct = oct_globalMax[i];

		if((maxOct - minOct) < MINIMUM_CAL_RANGE) {
			maxOct = minOct + MINIMUM_CAL_RANGE;
		}
        int pos = map(oct_averages[i],minOct,maxOct,1,BUCKET_SIZE);

        if(pos < 1)
          pos = 1;
        if(pos > 16)
          pos = 16;
        data[i].legNumber = i;
        data[i].value = pos;
      }
      clearAverages();
    }
    return clipped;
}


bool fht() { 	
	
	cli();
    bool clipped = readInput();
    fht_window(); // window the data for better frequency response
    fht_reorder(); // reorder the data before doing the fht
    fht_run(); // process the data in the fht
    fht_mag_octave(); // take the output of the fht
	sei();
    int avgCount  = countSinceAverage % AVERAGE_COUNT;
    if(countSinceAverage == GLOBAL_COUNT) {
      clearGlobal();
    }
    for(int i = 0; i < OCTAVES; i++) {
/*      if(oct_localMaxCalc[i] < fht_oct_out[i]) {
        oct_localMaxCalc[i] = fht_oct_out[i];
      }
      if (oct_localMinCalc[i] > fht_oct_out[i]) {
        oct_localMinCalc[i] = fht_oct_out[i];
      }*/
      if(oct_globalMax[i] < fht_oct_out[i]) {
        oct_globalMax[i] = fht_oct_out[i];
      }
      //if (oct_globalMin[i] > fht_oct_out[i]) {
      //  oct_globalMin[i] = fht_oct_out[i];
      //}
      oct_localAvgCalc[i] = oct_localAvgCalc[i] + ((float)(fht_oct_out[i]-oct_localAvgCalc[i]))/((float)avgCount+1.0);
//      oct_globalAvg[i] = oct_globalAvg[i] + ((float)(fht_oct_out[i]-oct_globalAvg[i]))/((float)countSinceAverage+1.0);  

    }
    if(avgCount == 0) {
      for(int i = 0; i < OCTAVES; i++) {
//        oct_localMin[i] = oct_localMinCalc[i];
//        oct_localMax[i] = oct_localMaxCalc[i];
        oct_localAvg[i] = oct_localAvgCalc[i];

      }
    }
    countSinceAverage++;
	return clipped;
}

void clearGlobal() {
  for(int i = 0; i < OCTAVES; i++) {
    oct_globalMax[i] = (oct_globalMax[i] + oct_localAvg[i])/2;
//    oct_globalMin[i] = (oct_globalMin[i] + oct_bias[i])/2;
//    memset(oct_globalAvg, 0, sizeof(int)*OCTAVES);
  }
  countSinceAverage = 0;
}

bool readInput() { 
//    beforeRead();
	bool clipped = false;
    int q;
    for (int i = 0 ; i < FHT_N ; i++) { // save 256 samples
  //    afterRead();
    // beforeRead();
      while(!(ADCSRA & 0x10)); // wait for adc to be ready
      ADCSRA = 0xf5; // restart adc
      byte m = ADCL; // fetch adc data
      byte j = ADCH;
      int k = (j << 8) | m; // form into an int
	  if(k == 0)
		  clipped = true;
      k -= readOffset; // form into a signed int
	  k <<= 6; // form into a 16b signed int 
      fht_input[i] = k; // put real data into bins 
   }
	return clipped;
}

uint8_t c0V;

void beforeRead() {
	c0V = TCNT0;
}

void afterRead() {
  int nv = TCNT0 - c0V;
  if(nv < 0)
    nv += 256;
  // 16mhz arduino ticks at 4us
  delayMicroseconds(US_BETWEEN_SAMPLES - (nv*4));
}

void clearAverages() { 
    memset(oct_averages,0,sizeof(int)*OCTAVES);
    count=0;
}

void averageOutput() {
  count++;
  for(int i = 0; i < OCTAVES; i++) {
      oct_averages[i] = oct_averages[i] + ((fht_oct_out[i]-oct_averages[i])/count);
      if(oct_globalMax[i] < oct_averages[i]) {
        oct_globalMax[i] = oct_averages[i];
      }
  }
}

void avgCalib(){
  clearAverages();
  for(int i = 0; i < AVERAGE_COUNT; i++) {
    fht();
	averageOutput();
  }

  for(int i = 0; i < OCTAVES; i++) {
    oct_globalMax[i] = oct_globalMax[i];
  }
}